#pragma once
#include "vector3.h"

namespace krryn{
namespace math{
	template<typename T>
	class quaternion{
	public:
		T m_X, m_Y, m_Z, m_W;

		quaternion()
			: m_X(0), m_Y(0), m_Z(0), m_W(0) {}

		quaternion(T a_X, T a_Y, T a_Z, T a_W)
			: m_X(a_X), m_Y(a_Y), m_Z(a_Z), m_W(a_W) {}

		static quaternion implicit_w(T a_X, T a_Y, T a_Z){
			T l_Temp = 1 - (a_X * a_X) - (a_Y * a_Y) - (a_Z * a_Z);

			if(l_Temp < 0) l_Temp = 0;
			else l_Temp = -math::sqrt(l_Temp);

			return quaternion(a_X, a_Y, a_Z, l_Temp);
		}
		
		quaternion normalize() const{
			T l_Length = math::sqrt((m_X * m_X) + (m_Y * m_Y) + (m_Z * m_Z) + (m_W * m_W));

			if(l_Length > 0){
				l_Length = 1 / l_Length;
				return quaternion(
					m_X * l_Length, 
					m_Y * l_Length, 
					m_Z * l_Length, 
					m_W * l_Length
				);
			}else{
				return quaternion::zero();
			}
		}

		static quaternion zero(){
			return quaternion(0, 0, 0, 0);
		}

		quaternion operator-() const{
			return quaternion(
				-m_X,
				-m_Y,
				-m_Z,
				 m_W // generally not inverted
			);
		}

		quaternion& operator=(const quaternion &a_Other){
			m_X = a_Other.m_X;
			m_Y = a_Other.m_Y;
			m_Z = a_Other.m_Z;
			m_W = a_Other.m_W;

			return *this;
		}

		quaternion& operator*=(const quaternion &a_Other){
			*this = *this * a_Other;
			return *this;
		}


		quaternion operator*(const quaternion &a_Other) const{
			return quaternion(
				(m_X * a_Other.m_W) + (m_W * a_Other.m_X) + (m_Y * a_Other.m_Z) - (m_Z * a_Other.m_Y),
				(m_Y * a_Other.m_W) + (m_W * a_Other.m_Y) + (m_Z * a_Other.m_X) - (m_X * a_Other.m_Z),
				(m_Z * a_Other.m_W) + (m_W * a_Other.m_Z) + (m_X * a_Other.m_Y) - (m_Y * a_Other.m_X),
				(m_W * a_Other.m_W) - (m_X * a_Other.m_X) - (m_Y * a_Other.m_Y) - (m_Z * a_Other.m_Z)
			);
		}

		template<typename _Tag>
		quaternion operator*(const vector3<T, _Tag> &a_Vector) const{
			return quaternion(
				  (m_W * a_Vector.m_X) + (m_Y * a_Vector.m_Z) - (m_Z * a_Vector.m_Y),
				  (m_W * a_Vector.m_Y) + (m_Z * a_Vector.m_X) - (m_X * a_Vector.m_Z),
				  (m_W * a_Vector.m_Z) + (m_X * a_Vector.m_Y) - (m_Y * a_Vector.m_X),
				- (m_X * a_Vector.m_X) - (m_Y * a_Vector.m_Y) - (m_Z * a_Vector.m_Z)
			);
		}

		template<typename _Tag>
		vector3<T, _Tag> rotate(const vector3<T, _Tag> &a_Vector) const{
			quaternion l_Inverse = (-*this).normalize();
			quaternion l_Temp    = *this * a_Vector;
			quaternion l_Result  = l_Temp * l_Inverse;

			return vector3<T, _Tag>(
				l_Result.m_X,
				l_Result.m_Y,
				l_Result.m_Z
			);
		}
	};

	typedef quaternion<float> quaternionf;
}
}